#include <stdio.h>
#include <sys/ioctl.h>
#include <termios.h>
#include <unistd.h>

#include <iostream>
#include <string>

#include <Eigen/Core>
#include <Eigen/StdVector>
#include <pangolin/pangolin.h>
#include <pangolin/display/display.h>
#include <pcl/common/transforms.h>
#include <pcl/conversions.h>
#include <pcl/io/pcd_io.h>
#include <pcl/point_cloud.h>

#include "custom_point_types.h"
#include "extrinsic_param.hpp"

#include "modules/common_msgs/sensor_msgs/pointcloud.pb.h"

#include "cyber/cyber.h"

using namespace std;

#define GL_GPU_MEM_INFO_CURRENT_AVAILABLE_MEM_NVX 0x9049
#define MAX_RADAR_TIME_GAP 15 * 1e6

pangolin::GlBuffer *source_vertexBuffer_;
pangolin::GlBuffer *source_colorBuffer_;
pangolin::GlBuffer *target_vertexBuffer_;
pangolin::GlBuffer *target_colorBuffer_;
int target_size_ = 0;
int child_size_ = 0;

string frame_id_, child_id_;

bool processing_ = false;
bool child_processing_ = false;

double cali_scale_degree_ = 0.3;
double cali_scale_trans_ = 0.06;
static Eigen::Matrix4d calibration_matrix_ = Eigen::Matrix4d::Identity();
static Eigen::Matrix4d orign_calibration_matrix_ = Eigen::Matrix4d::Identity();
std::vector<Eigen::Matrix4d, Eigen::aligned_allocator<Eigen::Matrix4d>>
    modification_list_;

std::unique_ptr<apollo::cyber::Node> listener_node_;
std::vector<ApolloPointXYZIT> target_cloud_;
std::vector<ApolloPointXYZIT> child_cloud_;

bool c_color_ = true;
int c_pointSize_ = 2;

int save_num_ = 0;

bool m_color_ = true;
int m_pointsize_ = 2;

bool pause_ = false;

struct RGB {
  unsigned char r;
  unsigned char g;
  unsigned char b;
};

void printCM() {
  for (int i = 0; i < 12; i++) {
    cout << " m list size: " << modification_list_.size() << "\n"
         << i << ":\n"
         << modification_list_[i] << endl;
  }
}

void CalibrationInit(Eigen::Matrix4d json_param) {
  Eigen::Matrix4d init_cali;
  init_cali << 1, 0, 0, 0, 0, 1, 0, 0, 0, 0, 1, 0, 0, 0, 0, 1;
  calibration_matrix_ = json_param;
  orign_calibration_matrix_ = json_param;
  modification_list_.reserve(12);
  for (int32_t i = 0; i < 12; i++) {
    std::vector<int> transform_flag(6, 0);
    transform_flag[i / 2] = (i % 2) ? (-1) : 1;
    Eigen::Matrix4d tmp = Eigen::Matrix4d::Identity();
    Eigen::Matrix3d rot_tmp;
    rot_tmp =
        Eigen::AngleAxisd(transform_flag[0] * cali_scale_degree_ / 180.0 * M_PI,
                          Eigen::Vector3d::UnitX()) *
        Eigen::AngleAxisd(transform_flag[1] * cali_scale_degree_ / 180.0 * M_PI,
                          Eigen::Vector3d::UnitY()) *
        Eigen::AngleAxisd(transform_flag[2] * cali_scale_degree_ / 180.0 * M_PI,
                          Eigen::Vector3d::UnitZ());
    tmp.block(0, 0, 3, 3) = rot_tmp;
    tmp(0, 3) = transform_flag[3] * cali_scale_trans_;
    tmp(1, 3) = transform_flag[4] * cali_scale_trans_;
    tmp(2, 3) = transform_flag[5] * cali_scale_trans_;

    modification_list_[i] = tmp;
  }
  std::cout << "=>Calibration scale Init!\n";
}

void CalibrationScaleChange() {
  Eigen::Matrix4d init_cali;
  init_cali << 1, 0, 0, 0, 0, 1, 0, 0, 0, 0, 1, 0, 0, 0, 0, 1;

  modification_list_.reserve(12);

  for (int32_t i = 0; i < 12; i++) {
    std::vector<int> transform_flag(6, 0);
    transform_flag[i / 2] = (i % 2) ? (-1) : 1;
    Eigen::Matrix4d tmp = Eigen::Matrix4d::Identity();
    Eigen::Matrix3d rot_tmp;
    rot_tmp =
        Eigen::AngleAxisd(transform_flag[0] * cali_scale_degree_ / 180.0 * M_PI,
                          Eigen::Vector3d::UnitX()) *
        Eigen::AngleAxisd(transform_flag[1] * cali_scale_degree_ / 180.0 * M_PI,
                          Eigen::Vector3d::UnitY()) *
        Eigen::AngleAxisd(transform_flag[2] * cali_scale_degree_ / 180.0 * M_PI,
                          Eigen::Vector3d::UnitZ());
    tmp.block(0, 0, 3, 3) = rot_tmp;
    tmp(0, 3) = transform_flag[3] * cali_scale_trans_;
    tmp(1, 3) = transform_flag[4] * cali_scale_trans_;
    tmp(2, 3) = transform_flag[5] * cali_scale_trans_;
    modification_list_[i] = tmp;
  }
  std::cout << "=>Calibration scale update done!\n";
}

void saveResult(const int &frame_index) {
  std::string file_name = child_id_ + "_" + frame_id_ +
                          "_extrinsic_" + std::to_string(frame_index) + ".yaml";

  SaveExtrinsicYaml(file_name, frame_id_, child_id_, calibration_matrix_);
}

RGB GreyToColorMix(int val) {
  int r, g, b;
  if (val < 128) {
    r = 0;
  } else if (val < 192) {
    r = 255 / 64 * (val - 128);
  } else {
    r = 255;
  }
  if (val < 64) {
    g = 255 / 64 * val;
  } else if (val < 192) {
    g = 255;
  } else {
    g = -255 / 63 * (val - 192) + 255;
  }
  if (val < 64) {
    b = 255;
  } else if (val < 128) {
    b = -255 / 63 * (val - 192) + 255;
  } else {
    b = 0;
  }
  RGB rgb;
  rgb.b = b;
  rgb.g = g;
  rgb.r = r;
  return rgb;
}

void DrawApolloTarget() {
  if (target_vertexBuffer_ != nullptr) delete (target_vertexBuffer_);
  if (target_colorBuffer_ != nullptr) delete (target_colorBuffer_);

  int pointsNum = target_cloud_.size();

  pangolin::GlBuffer *vertexbuffer = new pangolin::GlBuffer(
      pangolin::GlArrayBuffer, pointsNum, GL_FLOAT, 3, GL_DYNAMIC_DRAW);
  pangolin::GlBuffer *colorbuffer = new pangolin::GlBuffer(
      pangolin::GlArrayBuffer, pointsNum, GL_UNSIGNED_BYTE, 3, GL_DYNAMIC_DRAW);
  float *dataUpdate = new float[pointsNum * 3];
  unsigned char *colorUpdate = new unsigned char[pointsNum * 3];
  for (int ipt = 0; ipt < pointsNum; ipt++) {
    Eigen::Vector4d pointPos(target_cloud_[ipt].x, target_cloud_[ipt].y,
                             target_cloud_[ipt].z, 1.0);
    dataUpdate[ipt * 3 + 0] = pointPos.x();
    dataUpdate[ipt * 3 + 1] = pointPos.y();
    dataUpdate[ipt * 3 + 2] = pointPos.z();

    if (m_color_) {
      RGB colorFake = GreyToColorMix(target_cloud_[ipt].intensity);
      colorUpdate[ipt * 3 + 0] = static_cast<unsigned char>(colorFake.r);
      colorUpdate[ipt * 3 + 1] = static_cast<unsigned char>(colorFake.g);
      colorUpdate[ipt * 3 + 2] = static_cast<unsigned char>(colorFake.b);
    } else {
      for (int k = 0; k < 3; k++) {
        colorUpdate[ipt * 3 + k] =
            static_cast<unsigned char>(target_cloud_[ipt].intensity);
      }
    }
  }

  (vertexbuffer)->Upload(dataUpdate, sizeof(float) * 3 * pointsNum, 0);
  (colorbuffer)->Upload(colorUpdate, sizeof(unsigned char) * 3 * pointsNum, 0);
  delete dataUpdate;
  delete colorUpdate;

  target_size_ = pointsNum;
  target_vertexBuffer_ = vertexbuffer;
  target_colorBuffer_ = colorbuffer;
}

void DrawApolloChild() {
  if (source_vertexBuffer_ != nullptr) delete (source_vertexBuffer_);
  if (source_colorBuffer_ != nullptr) delete (source_colorBuffer_);
  int pointsNum = child_cloud_.size();

  pangolin::GlBuffer *vertexbuffer = new pangolin::GlBuffer(
      pangolin::GlArrayBuffer, pointsNum, GL_FLOAT, 3, GL_DYNAMIC_DRAW);
  pangolin::GlBuffer *colorbuffer = new pangolin::GlBuffer(
      pangolin::GlArrayBuffer, pointsNum, GL_UNSIGNED_BYTE, 3, GL_DYNAMIC_DRAW);

  float *dataUpdate = new float[pointsNum * 3];
  unsigned char *colorUpdate = new unsigned char[pointsNum * 3];
  for (int ipt = 0; ipt < pointsNum; ipt++) {
    Eigen::Vector4d pointPos(child_cloud_[ipt].x, child_cloud_[ipt].y,
                             child_cloud_[ipt].z, 1.0);
    Eigen::Vector4d trans_p = calibration_matrix_ * pointPos;
    dataUpdate[ipt * 3 + 0] = trans_p.x();
    dataUpdate[ipt * 3 + 1] = trans_p.y();
    dataUpdate[ipt * 3 + 2] = trans_p.z();

    if (c_color_) {
      RGB colorFake = GreyToColorMix(child_cloud_[ipt].intensity);
      colorUpdate[ipt * 3 + 0] = static_cast<unsigned char>(colorFake.r);
      colorUpdate[ipt * 3 + 1] = static_cast<unsigned char>(colorFake.b);
      colorUpdate[ipt * 3 + 2] = static_cast<unsigned char>(colorFake.g);
    } else {
      // red
      colorUpdate[ipt * 3 + 0] =
          static_cast<unsigned char>(child_cloud_[ipt].intensity);
      colorUpdate[ipt * 3 + 1] = 0;
      colorUpdate[ipt * 3 + 2] = 0;
    }
  }

  (vertexbuffer)->Upload(dataUpdate, sizeof(float) * 3 * pointsNum, 0);
  (colorbuffer)->Upload(colorUpdate, sizeof(unsigned char) * 3 * pointsNum, 0);

  delete dataUpdate;
  delete colorUpdate;

  child_size_ = pointsNum;
  source_vertexBuffer_ = vertexbuffer;
  source_colorBuffer_ = colorbuffer;
}

void mainPCCallback(const std::shared_ptr<apollo::drivers::PointCloud> &msg) {
  if (pause_) {
    return;
  }
  processing_ = true;
  int pcsize = msg->point().size();
  target_cloud_.clear();
  target_cloud_.reserve(pcsize);
  for (auto point : msg->point()) {
    ApolloPointXYZIT ap;
    ap.x = point.x();
    ap.y = point.y();
    ap.z = point.z();
    ap.intensity = point.intensity();
    target_cloud_.push_back(ap);
  }
  processing_ = false;
}

void childPCCallback(const std::shared_ptr<apollo::drivers::PointCloud> &msg) {
  child_processing_ = true;
  int pcsize = msg->point().size();
  child_cloud_.clear();
  child_cloud_.reserve(pcsize);
  for (auto point : msg->point()) {
    ApolloPointXYZIT ap;
    ap.x = point.x();
    ap.y = point.y();
    ap.z = point.z();
    ap.intensity = point.intensity();
    child_cloud_.push_back(ap);
  }
  child_processing_ = false;
}

void processpointcloud() {
  if (pause_) {
    return;
  }
  DrawApolloTarget();
  DrawApolloChild();
}

void cyberinit(
    const char *name,
    const string &maintopic = "/apollo/sensor/lidar16/back/PointCloud2",
    const string &childtopic = "/apollo/sensor/lidar16/left/PointCloud2") {
  apollo::cyber::Init(name);

  auto now = std::chrono::system_clock::now().time_since_epoch().count();
  srand(now);

  listener_node_ =
      apollo::cyber::CreateNode("lidar_lidar_calib_" + std::to_string(rand()));
  std::cout << "Create node: " << listener_node_.get()->Name() << std::endl;
  std::function<void(const std::shared_ptr<apollo::drivers::PointCloud> &)>
      main_pc_call = std::bind(&mainPCCallback, std::placeholders::_1);

  auto main_lidar = listener_node_->CreateReader<apollo::drivers::PointCloud>(
      maintopic, main_pc_call);

  auto child_lidar = listener_node_->CreateReader<apollo::drivers::PointCloud>(
      childtopic, [](const std::shared_ptr<apollo::drivers::PointCloud> &msg) {
        if (pause_) {
          return;
        }
        childPCCallback(msg);
      });

  cout << "Listeners created: " << maintopic << "\n" << childtopic << "\n";
}

void resetExtrinsic() {
  calibration_matrix_ = orign_calibration_matrix_;
  DrawApolloChild();
  std::cout << "Reset Func!\n";
}

void saveExtrinsic() {
  saveResult(save_num_);
  std::cout << "\n==>Save Result " << save_num_ << std::endl;
  Eigen::Matrix4d transform = calibration_matrix_;
  cout << "Transfromation Matrix:\n" << transform << std::endl;
  save_num_++;
}

void pauseStream() {
  pause_ = !pause_;
  if (pause_) {
    std::cout << "Paused!" << pause_ << "\n";
  } else {
    std::cout << "Continue!" << pause_ << "\n";
  }
}

int main(int argc, char **argv) {
  string main_lidar_topic;
  string child_lidar_topic;
  string extrinsic_json;

  if (argc == 2) {
    extrinsic_json = argv[1];
  } else if (argc != 4) {
    cout << "Usage 1: ./realtime_lidar2lidar <main_lidar_topic> "
            "<child_lidar_topic> "
            "<extrinsic_json>"
            "\nexample:\n\t"
            "./realtime_lidar2lidar /apollo/sensor/lidar16/back/PointCloud2 "
            "/apollo/sensor/lidar16/right/PointCloud2 "
            "data/rightlidar2back.json"
            "\nUsage 2: ./realtime_lidar2lidar <extrinsic_json>"
            "\nexample:\n\t"
            "./realtime_lidar2lidar data/rightlidar2back.json"
         << endl;
    return 0;
  } else {
    main_lidar_topic = argv[1];
    child_lidar_topic = argv[2];
    extrinsic_json = argv[3];
  }

  // load extrinsic
  Eigen::Matrix4d json_param;

  LoadExtrinsic(extrinsic_json, json_param, frame_id_, child_id_,
                main_lidar_topic, child_lidar_topic);

  cyberinit(argv[0], main_lidar_topic, child_lidar_topic);
  sleep(0.1);

  std::cout << "lidar to lidar extrinsic:\n" << json_param << std::endl;
  cout << "Using frame id: " << frame_id_ << " child id: " << child_id_;
  cout << "Cali_scale_degree_: " << cali_scale_degree_ << endl;
  cout << "Cali_scale_trans_: " << cali_scale_trans_ << endl;
  CalibrationInit(json_param);
  const int width = 1920, height = 1280;
  pangolin::CreateWindowAndBind("Lidar2Lidar Calibrator", width, height);

  glEnable(GL_DEPTH_TEST);
  glDepthMask(GL_TRUE);
  glDepthFunc(GL_LESS);

  pangolin::OpenGlRenderState s_cam(
      pangolin::ProjectionMatrix(1024, 768, 500, 500, 512, 389, 0.1, 1000),
      pangolin::ModelViewLookAt(-10, -5, 20, 0, 0, 0, pangolin::AxisZ));
  pangolin::View &d_cam =
      pangolin::CreateDisplay()
          .SetBounds(0.0, 1.0, 0.0, 1.0, -1.0 * width / height)
          .SetHandler(new pangolin::Handler3D(s_cam));
  glClearColor(0.0f, 0.0f, 0.0f, 0.0f);
  pangolin::OpenGlMatrix Twc; 
  Twc.SetIdentity();

  auto bottompanelheight = pangolin::Attach::Pix(21);
  // control panel
  pangolin::CreatePanel("mainlidar").SetBounds(0.9, 1.0, 0.0, 0.1);

  pangolin::CreatePanel("childlidar").SetBounds(0.9, 1.0, 0.9, 1.0);
  pangolin::CreatePanel("rotate").SetBounds(bottompanelheight, 0.3, 0.0, 0.1);
  pangolin::View &trans_panel = pangolin::CreatePanel("trans").SetBounds(bottompanelheight, 0.3, 0.9, 1.0);
  pangolin::CreatePanel("main")
      .SetBounds(0.0, bottompanelheight, 0.0, 1.0)
      .SetLayout(pangolin::LayoutEqualHorizontal);

  // main lidar
  pangolin::Var<string> m_label("mainlidar.Main Lidar Control");
  pangolin::Var<bool> m_colorMode("mainlidar.Intensity/Color (s)", m_color_, true);
  pangolin::Var<int> m_pointSize("mainlidar.Point Size", 2, 1, 8);

  // child lidar
  pangolin::Var<string> c_label("childlidar.Child Lidar Control");
  pangolin::Var<bool> c_colorMode("childlidar.Intensity/Color (l)", c_color_, true);
  pangolin::Var<int> c_pointSize("childlidar.Point Size", 2, 1, 8);

  pangolin::Var<string> rotate_label("rotate.Rotation Control");
  pangolin::Var<double> degreeStep("rotate.Step (degree)", 0.3, 0.01, 2.5);
  pangolin::Var<bool> addXdegree("rotate.+ x rotate (w)", false, false);
  pangolin::Var<bool> minusXdegree("rotate.- x rotate (x)", false, false);
  pangolin::Var<bool> addYdegree("rotate.+ y rotate (e)", false, false);
  pangolin::Var<bool> minusYdegree("rotate.- y rotate (z)", false, false);
  pangolin::Var<bool> addZdegree("rotate.+ z rotate (a)", false, false);
  pangolin::Var<bool> minusZdegree("rotate.- z rotate (d)", false, false);

  pangolin::Var<string> trans_label("trans.Translation Control");
  pangolin::Var<double> tStep("trans.Step (cm)", 5, 1, 20);
  pangolin::Var<bool> addXtrans("trans.+ x translate (p)", false, false);
  pangolin::Var<bool> minusXtrans("trans.- x translate (,)", false, false);
  pangolin::Var<bool> addYtrans("trans.+ y translate (k)", false, false);
  pangolin::Var<bool> minusYtrans("trans.- y translate (;)", false, false);
  pangolin::Var<bool> addZtrans("trans.+ z translate (o)", false, false);
  pangolin::Var<bool> minusZtrans("trans.- z translate (>)", false, false);

  pangolin::Button& b1 = (pangolin::Button&)trans_panel[7];
  b1.gltext = pangolin::GlFont::I().Text("- z translate (.)");

  std::vector<pangolin::Var<bool>> mat_calib_box;
  mat_calib_box.push_back(addXdegree);
  mat_calib_box.push_back(minusXdegree);
  mat_calib_box.push_back(addYdegree);
  mat_calib_box.push_back(minusYdegree);
  mat_calib_box.push_back(addZdegree);
  mat_calib_box.push_back(minusZdegree);
  mat_calib_box.push_back(addXtrans);
  mat_calib_box.push_back(minusXtrans);
  mat_calib_box.push_back(addYtrans);
  mat_calib_box.push_back(minusYtrans);
  mat_calib_box.push_back(addZtrans);
  mat_calib_box.push_back(minusZtrans);

  pangolin::Var<std::function<void()>> resetbutton("main.Reset Extrinsic (v)",
                                                   resetExtrinsic);
  pangolin::Var<std::function<void()>> pausebutton("main.Pause/Continue (b)",
                                                   pauseStream);
  pangolin::Var<std::function<void()>> savebutton("main.Save Result (n)",
                                                  saveExtrinsic);

  // register Key actions
  pangolin::RegisterKeyPressCallback('v', resetExtrinsic);
  pangolin::RegisterKeyPressCallback('b', pauseStream);
  pangolin::RegisterKeyPressCallback('n', saveExtrinsic);

  pangolin::RegisterKeyPressCallback('s', [&]() {
    m_color_ = !m_color_;
    m_colorMode = m_color_;
  });
  pangolin::RegisterKeyPressCallback('l', [&]() {
    c_color_ = !c_color_;
    c_colorMode = c_color_;
  });

  pangolin::RegisterKeyPressCallback('V', resetExtrinsic);
  pangolin::RegisterKeyPressCallback('B', pauseStream);
  pangolin::RegisterKeyPressCallback('N', saveExtrinsic);

  pangolin::RegisterKeyPressCallback('S', [&]() {
    m_color_ = !m_color_;
    m_colorMode = m_color_;
  });
  pangolin::RegisterKeyPressCallback('L', [&]() {
    c_color_ = !c_color_;
    c_colorMode = c_color_;
  });

  // +xr  -xr  +yr  -yr  +zr  -zr  +xt  -xt  +yt  -yt  +zt  -zt
  char keytable[] = {'w', 'x', 'e', 'z', 'a', 'd',
                     'p', ',', 'k', ';', 'o', '.'};
  char capskeytable[] = {'W', 'X', 'E', 'Z', 'A', 'D',
                     'P', '<', 'K', ':', 'O', '>'};

  for (int32_t i = 0; i < 12; i++) {
    pangolin::RegisterKeyPressCallback(keytable[i], [&, i]() {
      calibration_matrix_ = calibration_matrix_ * modification_list_[i];
    });
    pangolin::RegisterKeyPressCallback(capskeytable[i], [&, i]() {
      calibration_matrix_ = calibration_matrix_ * modification_list_[i];
    });
  }

  // viewMain.AddDisplay
  processpointcloud();
  sleep(0.1);

  int trigger_count = 0;
  while (!pangolin::ShouldQuit()) {
    s_cam.Follow(Twc);
    d_cam.Activate(s_cam);
    glClearColor(0.0f, 0.0f, 0.0f, 0.0f);
    glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT);

    if (c_colorMode.GuiChanged()) {
      c_color_ = c_colorMode;
    }

    if (m_colorMode.GuiChanged()) {
      m_color_ = m_colorMode;
    }

    if (c_pointSize.GuiChanged()) {
      c_pointSize_ = c_pointSize.Get() - 1;
      std::cout << "Child lidar point size changed to " << c_pointSize_ << "\n";
    }

    if (m_pointSize.GuiChanged()) {
      m_pointsize_ = m_pointSize.Get() - 1;
      std::cout << "Main lidar point size changed to " << m_pointsize_ << "\n";
    }

    if (degreeStep.GuiChanged()) {
      cali_scale_degree_ = degreeStep.Get();
      CalibrationScaleChange();
      std::cout << "Degree calib scale changed to " << cali_scale_degree_
                << " degree\n";
    }
    if (tStep.GuiChanged()) {
      cali_scale_trans_ = tStep.Get() / 100.0;
      CalibrationScaleChange();
      std::cout << "Trans calib scale changed to " << cali_scale_trans_ * 100
                << " cm\n";
    }
    for (int i = 0; i < 12; i++) {
      if (pangolin::Pushed(mat_calib_box[i])) {
        calibration_matrix_ = calibration_matrix_ * modification_list_[i];
        DrawApolloChild();
      }
    }

    // draw main coordinate
    float unitline = 3.0;
    glLineWidth(5);
    glBegin(GL_LINES);
    // x axis  red
    glColor3f(0.75, 0.0, 0.0);
    glVertex3d(0, 0, -1.7);
    glVertex3d(unitline, 0, -1.7);

    // y axis  green
    glColor3f(0.0, 0.75, 0.0);
    glVertex3d(0, 0, -1.7);
    glVertex3d(0, unitline, -1.7);

    // z axis  blue
    glColor3f(0.0, 0.0, 0.75);
    glVertex3d(0, 0, -1.7);
    glVertex3d(0, 0, unitline - 1.7);
    glEnd();

    // draw child coordinate

    Eigen::Vector4d childorigin(0.0, 0.0, 0.0, 1.0);
    Eigen::Vector4d childonmain = calibration_matrix_ * childorigin;
    double x0 = childonmain.x();
    double y0 = childonmain.y();
    double z0 = childonmain.z();

    float childunit = 2.0;
    Eigen::Vector4d childxline(childunit, 0.0, 0.0, 1.0);
    Eigen::Vector4d childxonmain = calibration_matrix_ * childxline;

    Eigen::Vector4d childyline(0.0, childunit, 0.0, 1.0);
    Eigen::Vector4d childyonmain = calibration_matrix_ * childyline;

    Eigen::Vector4d childzline(0.0, 0.0, childunit, 1.0);
    Eigen::Vector4d childzonmain = calibration_matrix_ * childzline;

    glLineWidth(5);
    glEnable(GL_LINE_STIPPLE);
    glLineStipple(2, 0xf00f);
    glBegin(GL_LINES);
    // x axis  deeppink #FF1493
    glColor3f(1.0, 0.08, 0.58);
    glVertex3d(x0, y0, z0);
    glVertex3d(childxonmain.x(), childxonmain.y(), childxonmain.z());

    // y axis  green -> springgreen #00ff7f
    glColor3f(0.0, 1.0, 0.5);
    glVertex3d(x0, y0, z0);
    glVertex3d(childyonmain.x(), childyonmain.y(), childyonmain.z());

    // z axis  blue -> cyan
    glColor3f(0.0, 0.5, 1.0);
    glVertex3d(x0, y0, z0);
    glVertex3d(childzonmain.x(), childzonmain.y(), childzonmain.z());
    glEnd();

    glDisable(GL_LINE_STIPPLE);

    // draw plane
    glBegin(GL_POLYGON);
    float unitedge = unitline / 2;
    // plane  yellow
    glColor3f(0.75, 0.75, 0.0);
    glVertex3d(0, 0, -1.7);
    glVertex3d(unitedge, 0, -1.7);
    glVertex3d(unitedge, unitedge, -1.7);
    glVertex3d(0, unitedge, -1.7);
    glEnd();

    // draw lidar points
    glDisable(GL_LIGHTING);
    glPointSize(c_pointSize_);

    // // draw child lidar points
    source_colorBuffer_->Bind();
    glColorPointer(source_colorBuffer_->count_per_element,
                   source_colorBuffer_->datatype, 0, 0);
    glEnableClientState(GL_COLOR_ARRAY);
    source_vertexBuffer_->Bind();
    glVertexPointer(source_vertexBuffer_->count_per_element,
                    source_vertexBuffer_->datatype, 0, 0);
    glEnableClientState(GL_VERTEX_ARRAY);
    glDrawArrays(GL_POINTS, 0, child_size_);
    glDisableClientState(GL_VERTEX_ARRAY);
    source_vertexBuffer_->Unbind();
    glDisableClientState(GL_COLOR_ARRAY);
    source_colorBuffer_->Unbind();

    // draw target lidar points

    glPointSize(m_pointsize_);

    target_colorBuffer_->Bind();
    glColorPointer(target_colorBuffer_->count_per_element,
                   target_colorBuffer_->datatype, 0, 0);
    glEnableClientState(GL_COLOR_ARRAY);
    target_vertexBuffer_->Bind();
    glVertexPointer(target_vertexBuffer_->count_per_element,
                    target_vertexBuffer_->datatype, 0, 0);
    glEnableClientState(GL_VERTEX_ARRAY);
    glDrawArrays(GL_POINTS, 0, target_size_);
    glDisableClientState(GL_VERTEX_ARRAY);
    target_vertexBuffer_->Unbind();
    glDisableClientState(GL_COLOR_ARRAY);
    target_colorBuffer_->Unbind();

    pangolin::FinishFrame();
    usleep(100);
    glFinish();
    while (processing_ || child_processing_) {
      sleep(0.01);
    }

    trigger_count++;
    if (trigger_count == 10) {
      processpointcloud();
      trigger_count = 0;
    }
  }

  Eigen::Matrix4d transform = calibration_matrix_;
  cout << "\nFinal Transfromation Matrix:\n" << transform << std::endl;

  apollo::cyber::AsyncShutdown();
  apollo::cyber::WaitForShutdown();

  return 0;
}
